from interbotix_xs_modules.xs_robot.turret import InterbotixTurretXS
def main():
    robot = InterbotixTurretXS(
        robot_model="pxxls",
        pan_profile_type="time",
        tilt_profile_type="time"
    )

    pan_position = 0.0
    tilt_position = 0.0
    step = 0.3

    pan_limits = (-3.14, 3.14)
    tilt_limits = (-1.57, 1.57)

    while True:
        command = input("Enter command (w/a/s/d to move, q to quit): ").strip().lower()

        if command == 's':
            tilt_position = min(tilt_position + step, tilt_limits[1])
            robot.turret.tilt(position=tilt_position, blocking=False)

        elif command == 'w':
            tilt_position = max(tilt_position - step, tilt_limits[0])
            robot.turret.tilt(position=tilt_position, blocking=False)

        elif command == 'd':
            pan_position = max(pan_position - step, pan_limits[0])
            robot.turret.pan(position=pan_position, blocking=False)

        elif command == 'a':
            pan_position = min(pan_position + step, pan_limits[1])
            robot.turret.pan(position=pan_position, blocking=False)

        elif command == 'q':
            break

        else:
            print("Invalid command. Use w/a/s/d to move, q to quit.")

    robot.shutdown()

if __name__ == '__main__':
    main()

